/**************************
 ***	ImageSubtruct.c	***
 *************************/

#include	"ImageType/ImageType.h"
#include	"Umath/LT2Type.h"



image_type *
image1_interpolation( image_type *sim,
					 float x0, float y0, int width, int height,
					 image_type *im )
{
u_char	*sp0,	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;
int	ix,	iy;
float	s1,	s2;
int	s;

	if( im != NULL && (im->column != width || im->row != height) ){
		image_destroy( im, 1 );
		im = NULL;
	}
	if( im == NULL )
		im = image_create( height, width, 1, 1, NULL );



	tp = (u_char *)im->data;
	for( i = 0, y = y0 ; i < im->row ; i++, y++ ){

		if( y < 0 || y >= sim->row-1){
			for( j = 0 ; j < im->column ; j++ )
				*tp++ = 0;
			continue;
		}


		iy = y;
		dy = y - iy;

		sp0 = IMAGE_PIXEL( sim, iy, 0 );

		for( j = 0, x = x0 ; j < im->column ; j++, x++ ){
			if( x < 0 || x >= sim->column-1 ){
				*tp++ = 0;
				continue;
			}

			ix = x;
			dx = x - ix;

			sp = sp0 + ix;

			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );
		}
	}

	return( im );
}

image_type *
image1_interpolation_1( image_type *sim,
					 float x0, float y0, int width, int height,
					 image_type *im )
{
u_char	*sp0,	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;
int	ix,	iy;
float	s1,	s2;
int	s;

	if( im != NULL && (im->column != width || im->row != height) ){
		image_destroy( im, 1 );
		im = NULL;
	}
	if( im == NULL )
		im = image_create( height, width, 1, 1, NULL );



	dy = y0 - (int)y0;
	if( y0 < 0 )	dy = -dy;

	dx = x0 - (int)x0;
	if( x0 < 0 )	dx = -dx;


	tp = (u_char *)im->data;
	for( i = 0, y = y0 ; i < im->row ; i++, y++ ){

		if( y < 0 || y > sim->row-1){
			for( j = 0 ; j < im->column ; j++ )
				*tp++ = 0;
			continue;
		}


		iy = y;
//		dy = y - iy;

		sp0 = IMAGE_PIXEL( sim, iy, 0 );

		for( j = 0, x = x0 ; j < im->column ; j++, x++ ){
			if( x < 0 || x > sim->column-1 ){
				*tp++ = 0;
				continue;
			}

			ix = x;
//			dx = x - ix;

			sp = sp0 + ix;

			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );
		}
	}

	return( im );
}



image_type *
image1_interpolationS( image_type *sim,
					 float x0, float y0, int width, int height,
					 float Dx, float Dy, float Ds,
					 image_type *im )
{
u_char	*sp0,	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;//,	ds;
int	ix,	iy;
float	s1,	s2;
int	s;

	if( im != NULL && (im->column != width || im->row != height) ){
		image_destroy( im, 1 );
		im = NULL;
	}
	if( im == NULL )
		im = image_create( height, width, 1, 1, NULL );


	tp = (u_char *)im->data;
	for( i = 0, y = y0 ; i < im->row ; i++ ){

		y = y0 + ( 1 + Ds ) * (i+0.5) + Dy;


		if( y < 0.5 || y > sim->row-1){
			for( j = 0 ; j < im->column ; j++ )
				*tp++ = 0;
			continue;
		}





		iy = y-0.5;
		dy = y - (iy+0.5);

		sp0 = IMAGE_PIXEL( sim, iy, 0 );

		for( j = 0, x = x0 ; j < im->column ; j++ ){

			x = x0 + ( 1 + Ds ) * (j+0.5) + Dx;

			if( x < 0.5 || x > sim->column-1 ){
				*tp++ = 0;
				continue;
			}

			
			ix = x-0.5;
			dx = x - (ix+0.5);

			sp = sp0 + ix;

			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );
		}
	}

	return( im );
}

image_type *
image1_interpolation_lt2( image_type *sim, lt2_type *lt,
					int x0, int y0,
					int width, int height,
					image_type *im )
{
u_char	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;
int	ix,	iy;
float	s1,	s2;
int	s,	no;


	im = image_recreate( im, height, width, 1, 1 );


	tp = (u_char *)im->data;
	for( i = 0, no = 0 ; i < im->row ; i++ ){

		for( j = 0 ; j < im->column ; j++ ){

			x = LT2_F1( *lt, j+0.5, i+0.5 ) + x0;
			y = LT2_F2( *lt, j+0.5, i+0.5 ) + y0;



			ix = x-0.5;
			iy = y-0.5;
	
			if( y < 0.5 || iy >= sim->row-1 ||  x < 0.5 || ix >= sim->column-1 ){
				ix = PUSH_TO_RANGE( ix, 0, sim->width-2 );
				iy = PUSH_TO_RANGE( iy, 0, sim->height-2 );
			}
			else	no++;




			//if( y < 0.5 || iy > sim->row-1-0.5 ||  x < 0.5 || x > sim->column-1-0.5 ){
			//	*tp++ = 0;
			//	continue;
			//}


			sp = IMAGE_PIXEL( sim, iy, ix );

			dx = x - (ix+0.5);
			dy = y - (iy+0.5);

			dx = PUSH_TO_RANGE( dx, 0, 1 );
			dy = PUSH_TO_RANGE( dy, 0, 1 );


			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );
		}
	}

	return( im );
}


int 
image1_interpolation_lt2_N( image_type *sim, lt2_type *lt,
					int x0, int y0,
					int width, int height,
					image_type **im )
{
u_char	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;
int	ix,	iy;
float	s1,	s2;
int	s,	no;


	*im = image_recreate( *im, height, width, 1, 1 );


	tp = (u_char *)(*im)->data;
	for( i = 0, no = 0 ; i < (*im)->row ; i++ ){

		for( j = 0 ; j < (*im)->column ; j++ ){

			x = LT2_F1( *lt, j+0.5, i+0.5 ) + x0;
			y = LT2_F2( *lt, j+0.5, i+0.5 ) + y0;



			ix = x-0.5;
			iy = y-0.5;
	
			if( y < 0.5 || iy >= sim->row-1 ||  x < 0.5 || ix >= sim->column-1 ){
				ix = PUSH_TO_RANGE( ix, 0, sim->width-2 );
				iy = PUSH_TO_RANGE( iy, 0, sim->height-2 );
			}
			else	no++;




			//if( y < 0.5 || iy > sim->row-1-0.5 ||  x < 0.5 || x > sim->column-1-0.5 ){
			//	*tp++ = 0;
			//	continue;
			//}


			sp = IMAGE_PIXEL( sim, iy, ix );

			dx = x - (ix+0.5);
			dy = y - (iy+0.5);

			dx = PUSH_TO_RANGE( dx, 0, 1 );
			dy = PUSH_TO_RANGE( dy, 0, 1 );


			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );
		}
	}

	return( 100*no/ (height* width)  );
}

int
image1M_interpolation_lt2( image_type *sim, lt2_type *lt,
						  int x0, int y0,
						  int width, int height,
						  image_type **im, image_type **mim )
{
	u_char	*sp,	*tp,	*mp;
	int	i,	j;
	float	x,	y,	dx,	dy;//,	ds;
	int	ix,	iy;
	float	s1,	s2;
	int	s;
	int	no;


	*im = image_recreate( *im, height, width, 1, 1 );
	*mim = image_recreate( *mim, height, width, 1, 1 );


	tp = (u_char *)(*im)->data;
	mp = (u_char *)(*mim)->data;

	for( i = 0, no = 0 ; i < (*im)->row ; i++ ){

		for( j = 0 ; j < (*im)->column ; j++ ){

			x = LT2_F1( *lt, j+0.5, i+0.5 ) + x0;
			y = LT2_F2( *lt, j+0.5, i+0.5 ) + y0;



			ix = x-0.5;
			iy = y-0.5;

			if( y < 0.5 || iy > sim->row-1-0.5 ||  x < 0.5 || x > sim->column-1-0.5 ){
				*tp++ = 0;
				*mp++ = 0;
				continue;
			}


			sp = IMAGE_PIXEL( sim, iy, ix );

			dx = x - (ix+0.5);
			dy = y - (iy+0.5);


			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );

			*mp = 256;

			no++;
		}
	}

	return( 100*no/ (height* width)  );
}




float
image1_interpolation_lt2_ssd( image_type *sim, lt2_type *lt,
							int x0, int y0,
							image_type *im, int j1, int i1,
							int width, int height )
{
u_char	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;
int	ix,	iy;
float	s1,	s2;
int	s;
float	tmp,	sum;
int	nSum,	align;


	sum = 0;
	nSum = 0;

	tp = (u_char *)im->data;
	tp = IMAGE_PIXEL( im, i1, j1 );
	align = im->width - width;


	for( i = 0 ; i < height ; i++, tp += align ){

		for( j = 0 ; j < width ; j++, tp++ ){

			x = LT2_F1( *lt, j+0.5, i+0.5 ) + x0;
			y = LT2_F2( *lt, j+0.5, i+0.5 ) + y0;



			ix = x-0.5;
			iy = y-0.5;


			//			if( iy < 0 || iy > sim->row-1 ||  ix < 0 || ix > sim->column-1 ){
			if( y < 0.5 || iy > sim->row-1-0.5 ||  x < 0.5 || x > sim->column-1-0.5 ){
				continue;
			}


			sp = IMAGE_PIXEL( sim, iy, ix );

			dx = x - (ix+0.5);
			dy = y - (iy+0.5);


			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;

			tmp = s  - (int) (*tp);
			sum += tmp*tmp;
			nSum ++;
		}
	}

	sum /= nSum;

	return( sum );
}



image_type *
image1_interpolation_Alt2( image_type *sim, lt2A_type *alt, float f, image_type *im )
{
u_char	*sp,	*tp;
int	i,	j;
float	x,	y,	dx,	dy;//,	ds;
int	ix,	iy;
float	s1,	s2;
int	s;


	im = image_recreate( im, sim->height, sim->width, 1, 1 );


	tp = (u_char *)im->data;
	for( i = 0 ; i < im->row ; i++ ){

		for( j = 0 ; j < im->column ; j++ ){


			lt2A_value( alt, (j+0.5)/f, (i+0.5)/f, &x, &y );

			x *= f;
			y *= f;



			ix = x-0.5;
			iy = y-0.5;


			//			if( iy < 0 || iy > sim->row-1 ||  ix < 0 || ix > sim->column-1 ){
			if( y < 0.5 || iy > sim->row-1-0.5 ||  x < 0.5 || x > sim->column-1-0.5 ){
				*tp++ = 0;
				continue;
			}


			sp = IMAGE_PIXEL( sim, iy, ix );

			dx = x - (ix+0.5);
			dy = y - (iy+0.5);


			s1 = (1-dx)* (*sp) + dx * (*(sp+1));
			sp += sim->column;
			s2 = (1-dx)* (*sp) + dx * (*(sp+1));

			s = (1-dy)*s1 + dy * s2 + 0.5;
			*tp++ = PUSH_TO_RANGE(s, 0, 255 );
		}
	}

	return( im );
}
